﻿#include "TesEuler.h"
#include "TesMatrix3x3.h"
#include "TesQuaternion.h"
#include <assert.h>

float& TesEular::operator[](int Index)
{
	assert(Index > 0 && Index < 3);
	if (Index == 0)
		return heading;
	else if (Index == 1)
		return pitch;
	return bank;
}

TesEular::TesEular()
{
	heading = pitch = bank = 0;
}

TesEular::TesEular(float p, float h, float b)
{
	assert((h >= -180.0f && h < 180.0f) && (p >= -90.0f && p <= 90.0f) && (b >= -180.0f && b <= 180.0f));
	heading = h;
	pitch = p;
	bank = b;
}

TesEular::TesEular(TesVector3 dir)
{
	//这里也跟左右手坐标系有很强的关系，这里默认为左手坐标系
	TesVector3 y(0.0f, 1.0f, 0.0f);
	TesVector3 z(0.0f, 0.0f, 1.0f);

	//计算x的偏转角(yoz平面)
	//注意x为-90 ~ 90
	float xAngle = 0.0f;
	if (dir.y != 0.0f)
	{
		TesVector3 yoz = dir;
		yoz.x = 0.0f;
		xAngle = acosf(yoz * y / (y.Length() * yoz.Length()));

		//表示是反向的旋转
		if (dir.z < 0)
		{
			xAngle = -xAngle;
			if (xAngle < (PAI / -2.0f))
				xAngle += (PAI / 2.0f);
		}
		else if (xAngle > (PAI / 2.0f))
			xAngle -= (PAI / 2.0f);
	}

	//计算y的偏转角度(xoz)
	float yAngle = 0.0f;
	if (dir.z != 0.0f)
	{
		TesVector3 xoz = dir;
		xoz.y = 0.0f;
		yAngle = acosf(xoz * z / (z.Length() * xoz.Length()));
		if (dir.x < 0)
			yAngle = -yAngle;
	}

	//计算z的偏转角度(xoy平面)
	float zAngle = 0.0f;
	if (dir.y != 0.0f)
	{
		TesVector3 xoy = dir;
		xoy.z = 0.0f;
		zAngle = acosf(xoy * y / (y.Length() * xoy.Length()));
		if (xoy.x > 0)	//这里为左手坐标系，如果是右手坐标系请使用[xoy.x < 0]为条件
			zAngle = -zAngle;
	}

	//弧度转化为角度
	xAngle = xAngle / PAI * 180.0f;
	yAngle = yAngle / PAI * 180.0f;
	zAngle = zAngle / PAI * 180.0f;

	pitch = xAngle;
	heading = yAngle;
	bank = zAngle;
}

TesMatrix3x3 TesEular::Matrix3x3()
{
	TesMatrix3x3 H = TesMatrix3x3();
	TesMatrix3x3 P = TesMatrix3x3();
	TesMatrix3x3 B = TesMatrix3x3();
	H.SetRotationYAxis(-heading / PAI);
	P.SetRotationXAxis(-pitch / PAI);
	B.SetRotationZAxis(-bank / PAI);
	return H * P * B;
}

TesQuaternion TesEular::Quaternion()
{
	TesQuaternion Res;
	float radianX = pitch / 180.0f * PAI;
	float radianY = heading / 180.0f * PAI;
	float radianZ = bank / 180.0f * PAI;
	Res.w = cosf(radianY * 0.5f) * cosf(radianX * 0.5f) * cosf(radianZ * 0.5f) + sinf(radianY * 0.5f) * sinf(radianX * 0.5f) * sinf(radianZ * 0.5f);
	Res.x = cosf(radianY * 0.5f) * sinf(radianX * 0.5f) * cosf(radianZ * 0.5f) + sinf(radianY * 0.5f) * cosf(radianX * 0.5f) * sinf(radianZ * 0.5f);
	Res.y = sinf(radianY * 0.5f) * cosf(radianX * 0.5f) * cosf(radianZ * 0.5f) - cosf(radianY * 0.5f) * sinf(radianX * 0.5f) * sinf(radianZ * 0.5f);
	Res.z = cosf(radianY * 0.5f) * cosf(radianX * 0.5f) * sinf(radianZ * 0.5f) - sinf(radianY * 0.5f) * sinf(radianX * 0.5f) * cosf(radianZ * 0.5f);
	return Res;
}

